/*
    Single touch Touchpad demonstration for the "4" on the Experimenter's board.

    Last updated: Feb 2007

    Author: Steve Underwood
*/

#include  <stdint.h>
#include  <stdlib.h>
#include  <msp430x20x3.h>

#define FALSE 0
#define TRUE (!FALSE)

/* This structure defines the static data to maintain one key sense line */
typedef struct
{
    unsigned char port;
    unsigned char port_bit;
    int threshold;
} key_line_config_data_t;

/* This structure defines the working data to maintain one key sense line */
typedef struct
{
    int base_capacitance;
    int filtered;
} key_line_data_t;

/* This is the adaption rate threshold. */
#define ADAPTION_RATE           1000

/* This is the ratio between the slower upwards adaption, and the faster
   downwards adaption. */
#define ADAPTION_RATIO          50

#define KEY_DOWN_THRESHOLD      12
#define KEY_UP_THRESHOLD        12

#define NUM_KEYS    	        16
#define NUM_LINES    	        8

/* The directional hysteresis is how many micro-steps the direction needs to appear to reverse by
   before we declare a change of direction. */
#define DIRECTIONAL_HYSTERESIS  6
//#define DIRECTIONAL_HYSTERESIS  12

/* The port bit allocation for the 8 key sensing lines */
const key_line_config_data_t key_line_config[NUM_LINES] =
{
    {1, BIT0, 40},
    {1, BIT1, 40},
    {1, BIT2, 40},
    {1, BIT3, 40},
    {1, BIT4, 40},
    {1, BIT5, 40},
    {2, BIT6, 40},
    {2, BIT7, 40},
};

/* The key sequence mapping to the 8 sense lines */
const uint8_t map_key_to_sense[NUM_KEYS] =
{
    2, 3, 1, 6,
    5, 2, 7, 6,
    1, 4, 3, 0,
    5, 4, 0, 7
};

/* The sense line mapping to the associated pairs of keys */
const uint8_t map_sense_to_key[NUM_KEYS][2] =
{
  {11, 14},
  {2, 8},
  {0, 5},
  {1, 10},
  {9, 13},
  {4, 12},
  {3, 7},
  {6, 15}
};

/* The working data for the 8 sense lines */
key_line_data_t key_line[NUM_LINES];

unsigned int timer_count;

void usi_i2c_init(void);
void send_to_host(int position);

uint8_t test_seq_data = 0;
uint8_t host_data = 0;

void init_key(key_line_data_t *key_line, const key_line_config_data_t *key_line_config)
{
    key_line->base_capacitance = 0;
    key_line->filtered = 0;

    /* Set the capacitive sense pin to output, low level, low going interupts */
    /* We need to keep all switches, except the one we are currently sensing,
       driven low so they form the "ground" against which the active pad and the
       finger form the variable capacitance. The end pads in the row can be
       expected to give a weaker response, because they are not surrounded by
       grounded keys on both sides, as the other keys are. */
    if (key_line_config->port == 1)
    {
        P1OUT &= ~key_line_config->port_bit;
        P1DIR |= key_line_config->port_bit;
        P1IES |= key_line_config->port_bit;
    }
    else
    {
        P2OUT &= ~key_line_config->port_bit;
        P2DIR |= key_line_config->port_bit;
        P2IES |= key_line_config->port_bit;
    }
}

unsigned int measure_key_capacitance(int key_no)
{
    char active_key;
    const key_line_config_data_t *keyp;
    const key_line_config_data_t *partner;
    int sum;

    keyp = &key_line_config[key_no];
    partner = &key_line_config[key_no ^ 1];

    /* Right now, all keys should be driven low, forming a "ground" area */
    active_key = keyp->port_bit;
    /* Take the active key high to charge the pad */
    if (keyp->port == 1)
        P1OUT |= active_key;
    else
        P2OUT |= active_key;
    /* Allow a short time for the hard pull high to really charge the pad */
    _NOP();
    _NOP();
    _NOP();
    /* Enable interrupts (edge is already set to low going trigger)
       set the active key to input (was output high), and start the
       timed discharge of the pad. */
    if (keyp->port == 1)
    {
        P1IES |= active_key;
        P1IE |= active_key;
        P1DIR &= ~active_key;
    }
    else
    {
        P2IES |= active_key;
        P2IE |= active_key;
        P2DIR &= ~active_key;
    }
    /* Take a snaphot of the timer... */
    timer_count = TAR;
    /* ...and wait for the discharge to cause an interrupt. We wait in LPM0 mode, as we
       need the fast clock running to perform accurate timing of the discharge. We
       need to minimise noise disturbing the discharge time. We should, therefore, have
       all unnecessary peripherals idle. We do not want other interrupts spoiling the
       crispness of our reponse to the port interrupt, so all other interrupts should
       be turned off at this time. */
    LPM0;
    /* Return the key to the driven low state, to contribute to the "ground" area
       around the next key to be scanned. */
    if (keyp->port == 1)
    {
	P1IE &= ~active_key;            /* disable active key interrupt */
        P1OUT &= ~active_key;           /* switch active key to low to discharge the key */
    	P1DIR |= active_key;            /* switch active key to output low to save power */
    }
    else
    {
	P2IE &= ~active_key;            /* disable active key interrupt */
    	P2OUT &= ~active_key;           /* switch active key to low to discharge the key */
    	P2DIR |= active_key;            /* switch active key to output low to save power */
    }
    sum = timer_count;
    /* Right now, all keys should be driven low, forming a "ground" area */
    active_key = keyp->port_bit;
    /* Take the partner key high to charge the pad */
    if (partner->port == 1)
        P1OUT |= partner->port_bit;
    else
        P2OUT |= partner->port_bit;
    /* Allow a short time for the hard pull high to really charge the pad */
    _NOP();
    _NOP();
    _NOP();
    /* Enable interrupts (edge is already set to low going trigger)
       set the active key to input (was output high), and start the
       timed discharge of the pad. */
    if (keyp->port == 1)
    {
        P1IES &= ~active_key;
        P1IE |= active_key;
        P1DIR &= ~active_key;
    }
    else
    {
        P2IES &= ~active_key;
        P2IE |= active_key;
        P2DIR &= ~active_key;
    }
    /* Take a snaphot of the timer... */
    timer_count = TAR;
    /* ...and wait for the discharge to cause an interrupt. We wait in LPM0 mode, as we
       need the fast clock running to perform accurate timing of the discharge. We
       need to minimise noise disturbing the discharge time. We should, therefore, have
       all unnecessary peripherals idle. We do not want other interrupts spoiling the
       crispness of our reponse to the port interrupt, so all other interrupts should
       be turned off at this time. */
    LPM0;
    /* Return the key to the driven low state, to contribute to the "ground" area
       around the next key to be scanned. */
    if (keyp->port == 1)
    {
	P1IE &= ~active_key;            /* disable active key interrupt */
        P1OUT &= ~active_key;           /* switch active key to low to discharge the key */
    	P1DIR |= active_key;            /* switch active key to output low to save power */
    }
    else
    {
	P2IE &= ~active_key;            /* disable active key interrupt */
    	P2OUT &= ~active_key;           /* switch active key to low to discharge the key */
    	P2DIR |= active_key;            /* switch active key to output low to save power */
    }
    if (partner->port == 1)
        P1OUT &= ~partner->port_bit;
    else
        P2OUT &= ~partner->port_bit;
    sum += timer_count;
    return sum >> 1;
}

int scan_keys(void)
{
    int i;
    int margin;

    for (i = 0;  i < NUM_LINES;  i++)
    {
        margin = measure_key_capacitance(i) - key_line[i].base_capacitance;
        key_line[i].filtered += (margin - (key_line[i].filtered >> 4));
    }
    return 0;
}

int peak = -100;

long int aaa;
long int bbb;
int min;
int grass_level;

void xxxx(int line, int key)
{
    if ((key_line[line].filtered - min) > grass_level)
    {
        aaa += (key_line[line].filtered - min);
        bbb += key*(key_line[line].filtered - min);
    }
}


int find_finger_position
 (
    int   last_peak,
    int*  new_peak
)
{
    int i;
    int j;
    int k;
    int l;
    long int a;
    long int b;
    int max;
    int max_pos;

    /* Find the minimum and maximum responses for the key lines */
    min = 32767;
    max = -32768;
    max_pos = -1;
    for (i = 0;  i < NUM_LINES;  i++)
    {
        if (key_line[i].filtered < min)
            min = key_line[i].filtered;
        if (key_line[i].filtered > max)
        {
            max = key_line[i].filtered;
            max_pos = i;
        }
    }
    /* If the maximum response isn't that big, there is no finger present. */
    if (max < 200)    //higher threshold for sheathed touchpad
      return -1;
    
    /* We have a finger */

    /* Now set a grass cutting level, to remove small noise values from the
       fine position estimation. */
    grass_level = (max - min) >> 3;
    
    /* We need to determine which of the two possible areas of the pad the finger is in, based on the
       maximum response. */
    /* Find the two possible positions */
    i = map_sense_to_key[max_pos][0];
    j = map_sense_to_key[max_pos][1];
    /* Try to make the finger flow, as a way to work out which of two possible areas is the real one */
    /* Treat vertical movement down through the cross specially */
    if (abs(peak - j) > abs(peak - i))
    {
        if (peak < 12  ||  i != 2)
            j = i;
    }
    aaa = 0;
    bbb = 0;
    if (peak >= 12  &&  i == 2)
    {
        /* We think the finger is moving vertically into the centre of the cross. This needs to be
           handled specially. */
        *new_peak = 13;
        xxxx(4, 13);
        xxxx(1, 14);
        xxxx(0, 15);
    }
    else
    {
        /* Do a linear weighted average around the area where we think the finger is */
        *new_peak = j;
        k = j - 1;
        if (k < 0)
            k = 0;
        l = j + 1;
        if (l >= NUM_KEYS)
            l = NUM_KEYS - 1;
        for (i = k;  i <= l;  i++)
        {
            j = map_key_to_sense[i];
            xxxx(j, i + 1);
        }
    }
    a = aaa;
    b = bbb;

    b /= (a >> 4);
    b -= 16;
    if (b < 0)
        b = 0;
    return b;
}

int last_position = 0;
int new_position = 0;
int button_down = 0;

void mySleepFunction(void)
{
  int i = 0;
  int val = 0;
  for (i = 0; i < 30000; ++i)
  {
    val = 1 +  2*i;
  }
  return;
}

int myFuncForIncDec(void)
{
    int i;
    int valueToDisplay = 0;

    int last_peak = 0;
    int new_peak = 0;
    
    WDTCTL = WDTPW | WDTHOLD;             /* Stop the watchdog */

    /* Use the VLOCLK oscillator. This is important. If we leave the unused 32kHz
       oscillator running, its pins will not be free for sensing. */
    BCSCTL3 = LFXT1S_2;
    BCSCTL1 = CALBC1_16MHZ;               /* Set the DCO speed */
    DCOCTL = CALDCO_16MHZ;

    /* We must turn off the low frequency crystal oscillator, as we are using its pins as I/O pins */
    _BIS_SR(OSCOFF);

    /* Initialise all the I/O pins */
    P1OUT = 0;
    P1DIR = 0xFF;
    P1SEL = 0;
    P1REN = 0;
    P2OUT = 0;
    P2DIR = 0xFF;
    P2SEL = 0;
    P2REN = 0;

    /* Prepare for I2C communication */
    usi_i2c_init();

    _EINT();

#if 0
    /* Send incrementing numbers to the host, to check out communications */
    for (;;)
    {
        send_to_host(test_seq_data++);
        /* A little delay between communication cycles */
        for (i = 0; i < 15000; i++)
            _NOP();
    }
#endif

    /* Initialise all the keys */
    for (i = 0;  i < NUM_LINES;  i++)
        init_key(&key_line[i], &key_line_config[i]);

    /* Drive Timer A from the SMCLK, in continuous mode */
    TACTL = TASSEL_2 | MC_2;          // used higher TAR clock freq. if 
                                      // sheathed touchpad is used.
    
    /* Scan the keys quite a few times, to allow plenty of time for the
       MCLK and board conditions to stablise */
    for (i = 0;  i < 100;  i++)
        scan_keys();

    /* Now we can use the current filtered key response as the base response. */
    for (i = 0;  i < NUM_LINES;  i++)
    {
        key_line[i].base_capacitance = key_line[i].filtered >> 4;
        key_line[i].filtered = 0;
    }

    for (;;)
    {
        scan_keys();
        
        new_position = find_finger_position
        (
           last_peak,
           &new_peak
        );
        
        //if ((new_position = find_finger_position()) >= 0) 
        if (new_position >= 0)
        {
            /* There is a finger on the pad */
            /* Apply some hysteresis to finger movement, so avoid jitter */
            if  (new_position > last_position + DIRECTIONAL_HYSTERESIS
                ||
                new_position < last_position - DIRECTIONAL_HYSTERESIS )
            {
                if (new_position > last_position)
                    last_position = new_position - (DIRECTIONAL_HYSTERESIS - 1);
                else
                    last_position = new_position + (DIRECTIONAL_HYSTERESIS - 1);
            }
        }
        else
        {
            /* There is no finger on the pad */
            last_position = -100;
        }   
        
        if (new_position >= 0)
        {   
          if ((new_peak >= last_peak) && (new_position >= last_position))
          {
            ++valueToDisplay;
          }
          else
           /* ((new_peak < last_peak) && (new_position > last_position))*/
          {
            --valueToDisplay;
          }
        }
        
        mySleepFunction();
        send_to_host(valueToDisplay);
        
        /*  store the value for next run */
        last_peak = new_peak;   
        
    } // end of for(;;)
  return 0;
}



void testMySleepFunc(void)
{
  int testDisplay = 1; 
  send_to_host(testDisplay);
  mySleepFunction();
  testDisplay =7;
  send_to_host(testDisplay);
  
  return;
}

void main(void)
{
   //testMySleepFunc(); 
   int success =  myFuncForIncDec(); 
} 

/* ----------------------------- KEY HANDLING INTERRUPTS ---------------------------- */

#pragma vector=PORT1_VECTOR
__interrupt void port_1_interrupt(void)
{
    P1IFG = 0;                                /* Clear interrupt flag */
    timer_count = TAR - timer_count;          /* Record the discharge time */
    LPM3_EXIT;                                /* Exit from low power 3 or 0 */
}

#pragma vector=PORT2_VECTOR
__interrupt void port_2_interrupt(void)
{
    P2IFG = 0;                                /* Clear interrupt flag */
    timer_count = TAR - timer_count;          /* Record the discharge time */
    LPM3_EXIT;                                /* Exit from low power 3 or 0 */
}

#pragma vector=TIMERA0_VECTOR
__interrupt void timera0_interrupt(void)
{
    LPM3_EXIT;
}

#pragma vector=TIMERA1_VECTOR
__interrupt void timera1_interrupt(void)
{
    switch (TAIV)
    {
    case 2:
        /* TACCR1 */
        LPM3_EXIT;
        break;
    }
}

/* ------------------------------- COMMUNICATIONS ---------------------- */

const uint8_t SLV_Addr = 0x48;                      /* I2C slave address is 0x48 */
int8_t I2C_state = -2;                               /* I2C state tracking */

void send_to_host(int data)
{
    host_data = data;
    I2C_state = 0;
    USICTL1 |= USIIFG;                              /* Set flag and start communication */
    /* Wait until the I/O operation has completed */
    do
        LPM0;
    while (I2C_state >= 0);
}

void usi_i2c_init(void)
{
    P1OUT |= (BIT7 | BIT6);                         /* P1.6 & P1.7 are for I2C */
    P1REN |= (BIT7 | BIT6);                         /* Enable P1.6 & P1.7 pullups */

    USICTL0 = USIPE6 | USIPE7 | USIMST | USISWRST;  /* Port & USI mode setup */
    USICTL1 = USII2C | USIIE;                       /* Enable I2C mode & USI interrupt */
    USICKCTL = USIDIV_7 | USISSEL_2 | USICKPL;      /* Setup USI clock: SCL = SMCLK/128 (~125kHz) */
    USICNT |= USIIFGCC;                             /* Disable automatic clear control */
    USICTL0 &= ~USISWRST;                           /* Enable USI */
    USICTL1 &= ~USIIFG;                             /* Clear pending flag */
}

#pragma vector = USI_VECTOR
__interrupt void USI_TXRX(void)
{
    switch (__even_in_range(I2C_state, 10))
    {
    case 0:
         /* Generate start condition & send address to slave */
        P1OUT |= 0x01;                      /* LED on: sequence start */
        USISRL = 0x00;                      /* Generate Start Condition... */
        USICTL0 |= (USIGE | USIOE);
        USICTL0 &= ~USIGE;
        USISRL = SLV_Addr << 1;             /* ... and transmit address, R/W = 0 */
        USICNT = (USICNT & 0xE0) + 0x08;    /* Bit counter = 8, TX Address */
        I2C_state = 2;                      /* Go to next state: receive address (N)Ack */
        break;
    case 2:
        /* Receive sddress Ack/Nack bit */
        USICTL0 &= ~USIOE;                  /* SDA = input */
        USICNT |= 0x01;                     /* Bit counter = 1, receive (N)Ack bit */
        I2C_state = 4;                      /* Go to next state: check (N)Ack */
        break;
    case 4:
        /* Process sddress Ack/Nack & handle data TX */
        USICTL0 |= USIOE;                   /* SDA = output */
        if (USISRL & 0x01)
        {
            /* Nack received. Send stop... */
            USISRL = 0x00;
            USICNT |=  0x01;                /* Bit counter = 1, SCL high, SDA low */
            I2C_state = 10;                 /* Go to next state: generate Stop */
            P1OUT |= 0x01;                  /* Turn on LED: error */
        }
        else
        {
            /* Ack received, TX data to slave... */
            USISRL = host_data;             /* Load data byte */
            USICNT |=  0x08;                /* Bit counter = 8, start TX */
            I2C_state = 6;                  /* Go to next state: receive data (N)Ack */
            P1OUT &= ~0x01;                 /* Turn off LED */
        }
        break;
    case 6:
        /* Receive Data Ack/Nack bit */
        USICTL0 &= ~USIOE;                  /* SDA = input */
        USICNT |= 0x01;                     /* Bit counter = 1, receive (N)Ack bit */
        I2C_state = 8;                      /* Go to next state: check (N)Ack */
        break;
    case 8:
        /* Process Data Ack/Nack & send Stop */
        USICTL0 |= USIOE;
        if (USISRL & 0x01)
        {
            /* Nack received... */
            /* TODO: Handle the NACK */
        }
        else
        {
            /* Ack received */
        }
        /* Send stop... */
        USISRL = 0x00;
        USICNT |=  0x01;                    /* Bit counter = 1, SCL high, SDA low */
        I2C_state = 10;                     /* Go to next state: generate stop */
        break;
    case 10:
        /* Generate Stop Condition */
        USISRL = 0x0FF;                     /* USISRL = 1 to release SDA */
        USICTL0 |= USIGE;                   /* Transparent latch enabled */
        USICTL0 &= ~(USIGE | USIOE);        /* Latch/SDA output disabled */
        I2C_state = -2;                     /* Reset state machine for next transmission */
        LPM0_EXIT;                          /* Exit active for next transfer */
        break;
    }
    USICTL1 &= ~USIIFG;                     /* Clear pending flag */
}

